/*********************************************************************
 * Software License Agreement (Modified BSD License)
 *
 * Copyright (c) 2010, Daniel Munoz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of the copyright holders' organizations nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include <submodular_graphcut/graphcut_bgl.h>

using namespace submodular_graphcut;
using namespace boost;

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
GraphcutBGL::GraphcutBGL()
{
  m_graph = new Graph();
  m_source = add_vertex(*m_graph);
  m_sink = add_vertex(*m_graph);
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
unsigned int GraphcutBGL::getValue(const EnergyVar& x)
{
  // 0 = source
  // 1 = sink
  property_map<Graph, vertex_color_t>::type v_colors = get(vertex_color, *m_graph);

  // From Boost documentation:
  // "If the color of a vertex after running the algorithm is black the vertex 
  //  belongs to the source tree else it belongs to the sink-tree."
  if (v_colors[x] == boost::black_color)
  {
    return 0;
  }
  else
  {
    return 1;
  }
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
void GraphcutBGL::add_edge(const EnergyVar& from,
                           const EnergyVar& to,
                           double cap_val,
                           double reverse_cap_val)
{
  property_map<Graph, edge_capacity_t>::type capacity = get(edge_capacity, *m_graph);
  property_map<Graph, edge_reverse_t>::type rev = get(edge_reverse, *m_graph);

  Traits::edge_descriptor e1;
  Traits::edge_descriptor e2;
  bool in1 = false;
  bool in2 = false;

  tie(e1, in1) = boost::add_edge(from, to, *m_graph);
  tie(e2, in2) = boost::add_edge(to, from, *m_graph);

  // if an edge already exists, augment capacity
  if (in1 == false)
  {
    capacity[e1] += cap_val;
  }
  else
  {
    capacity[e1] = cap_val;
    rev[e1] = e2;
  }
  if (in2 == false)
  {
    capacity[e2] += reverse_cap_val;
  }
  else
  {
    capacity[e2] = reverse_cap_val;
    rev[e2] = e1;
  }
}

// --------------------------------------------------------------
/* See function definition */
// --------------------------------------------------------------
void GraphcutBGL::add_tweights(const EnergyVar& x,
                               double cap_source_to_x,
                               double cap_x_to_sink)
{
  // Make negative weights/capacities to be positive
  // This will not affect the solution of the variables,
  // but will change the value, so we need to offset.
  // (Subtract a little due to avoid precision errors)
  double min_val = std::min(cap_source_to_x, cap_x_to_sink) - 1e-5;
  if (min_val < 0.0)
  {
    cap_source_to_x -= min_val;
    cap_x_to_sink -= min_val;
    m_const_offset += min_val;

    // Debug: add extra to see flow over all edges
    //cap_source_to_x += 1.0;
    //cap_x_to_sink += 1.0;
    //const_offset_ -= 1.0;
  }

  add_edge(m_source, x, cap_source_to_x, 0.0);
  add_edge(x, m_sink, cap_x_to_sink, 0.0);
}
